#include "RDB.h"

void RDB_MSG_NEED::clear() {
    dynamicObjects.clear();
    staticObjects.clear();
    roadPos.clear();
    laneInfo.clear();
    roadMark.clear();
    contactPoint.clear();
    trafficSign.clear();
    roadState.clear();
    environment.clear();
    driverCtrl.clear();
    trafficLight.clear();
}

//决策模块接口()
std::tuple<double, double, double> decision(const RDB_MSG_NEED& rdbMsgNeed) {
    return std::make_tuple(0.0, 0.0, 0.0);
}

//解析包头
void parseRDBMessage(RDB_MSG_t* msg, bool& isImage, 
            bool& sParseMessageEntry, RDB_MSG_NEED& rdbMsgNeed)
{
    if (!msg) return;

    if (!msg->hdr.dataSize) return;
    //去掉了消息头,一个消息可以有多个包
    RDB_MSG_ENTRY_HDR_t* entry = (RDB_MSG_ENTRY_HDR_t*)(((char*) msg) + msg->hdr.headerSize);
    uint32_t remainingBytes = msg->hdr.dataSize;
        
    while (remainingBytes)
    {
        if (parseRDBMessageEntry(msg->hdr.simTime, msg->hdr.frameNo, 
                        entry, sParseMessageEntry, rdbMsgNeed) == 1) {
            std::cout << ">>>>>>>>>>  完整消息输出  <<<<<<<<<<" << std::endl;
        }

        isImage |= (entry->pkgId == RDB_PKG_ID_IMAGE);

        remainingBytes -= (entry->headerSize + entry->dataSize);
        
        if (remainingBytes)
          entry = (RDB_MSG_ENTRY_HDR_t*)((((char*) entry) + entry->headerSize + entry->dataSize));
        std::cout << "----------  包头包体减一  ----------" << std::endl;
    }
}

//解析每个包，一个包可能包含一条信息或多条同样大小的信息
int parseRDBMessageEntry(const double& simTime, const unsigned int& simFrame, 
    RDB_MSG_ENTRY_HDR_t* entryHdr, bool& sParseMessageEntry, RDB_MSG_NEED& rdbMsgNeed)
{
    if ( !entryHdr )
        return -1;
    
    int noElements = entryHdr->elementSize ? ( entryHdr->dataSize / entryHdr->elementSize ) : 0;
    
    if ( !noElements )  // some elements require special treatment
    {
        switch ( entryHdr->pkgId )
        {
            case RDB_PKG_ID_START_OF_FRAME:
                std::cout << "void parseRDBMessageEntry: got start of frame" << std::endl;
                // std::cout << "sParseMessageEntry: " << sParseMessageEntry << std::endl;
                sParseMessageEntry = true;
                return 0;
                
            case RDB_PKG_ID_END_OF_FRAME:
                std::cout << "void parseRDBMessageEntry: got end of frame" << std::endl;
                sParseMessageEntry = false;
                return 1;
                
            default:
                break;
        }
        return -1;
    }

    unsigned char ident   = 6;
    char*         dataPtr = ( char* ) entryHdr;
        
    dataPtr += entryHdr->headerSize;
        
    while ( noElements-- )
    {
        bool printedMsg = true;
            
        switch ( entryHdr->pkgId )
        {
            /*
            case RDB_PKG_ID_COORD_SYSTEM:
                std::cout << "RDB_COORD_SYSTEM_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_COORD:
                std::cout << "RDB_COORD_t" << std::endl;
                break;
            */
            //给定实体的详细道路位置
            case RDB_PKG_ID_ROAD_POS:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_ROAD_POS_t*) dataPtr), rdbMsgNeed);
                }
                break;

            //给定实体的车道信息
            case RDB_PKG_ID_LANE_INFO:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_LANE_INFO_t*) dataPtr), rdbMsgNeed);
                }
                break;

            //交通车（通常是主车）的道路标记信息
            case RDB_PKG_ID_ROADMARK:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_ROADMARK_t*) dataPtr), rdbMsgNeed);
                }
                break;
            /*      
            case RDB_PKG_ID_OBJECT_CFG:
                std::cout << "RDB_OBJECT_CFG_t" << std::endl;
                break;
            */
            //标准或静态对象的状态
            case RDB_PKG_ID_OBJECT_STATE:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_OBJECT_STATE_t*) dataPtr), 
                        entryHdr->flags& RDB_PKG_FLAG_EXTENDED, rdbMsgNeed);
                }
                break;
            /* 
            case RDB_PKG_ID_VEHICLE_SYSTEMS:
                std::cout << "RDB_VEHICLE_SYSTEMS_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_VEHICLE_SETUP:
                std::cout << "RDB_VEHICLE_SETUP_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_ENGINE:
                std::cout << "RDB_ENGINE_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_DRIVETRAIN:
                std::cout << "RDB_DRIVETRAIN_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_WHEEL:
                std::cout << "RDB_WHEEL_t" << std::endl;
                break;

            case RDB_PKG_ID_PED_ANIMATION:
                std::cout << "RDB_PED_ANIMATION_t" << std::endl;
                break;

            case RDB_PKG_ID_SENSOR_STATE:
                std::cout << "RDB_SENSOR_STATE_t" << std::endl;
                break;

            case RDB_PKG_ID_SENSOR_OBJECT:
                std::cout << "RDB_SENSOR_OBJECT_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_CAMERA:
                std::cout << "RDB_CAMERA_t" << std::endl;
                break;
            */
            //给定接触点的道路信息
            case RDB_PKG_ID_CONTACT_POINT:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_CONTACT_POINT_t*) dataPtr), rdbMsgNeed);
                }
                break;
            //关于交通标志对象的信息
            case RDB_PKG_ID_TRAFFIC_SIGN:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_TRAFFIC_SIGN_t*) dataPtr), rdbMsgNeed);
                }
                break;
            //给定交通参与者的道路状态信息
            case RDB_PKG_ID_ROAD_STATE:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_ROAD_STATE_t*) dataPtr), rdbMsgNeed);
                }
                break;
            /*  
            case RDB_PKG_ID_IMAGE:
                
            case RDB_PKG_ID_LIGHT_MAP: 
                // std::cout << "RDB_IMAGE_t" << std::endl;
                // handleRDBitem( simTime, simFrame, *( ( RDB_IMAGE_t* ) dataPtr ) );
                break;

            case RDB_PKG_ID_LIGHT_SOURCE:
                std::cout << "RDB_LIGHT_SOURCE_t" << std::endl;
                break;
            */
            //环境信息
            case RDB_PKG_ID_ENVIRONMENT:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_ENVIRONMENT_t*) dataPtr), rdbMsgNeed);
                }
                break;
            /*        
            case RDB_PKG_ID_TRIGGER:
                std::cout << "RDB_TRIGGER_t" << std::endl;
                break;
            */
            //来自模型或驱动模块的动态输入信息
            case RDB_PKG_ID_DRIVER_CTRL:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_DRIVER_CTRL_t*) dataPtr), rdbMsgNeed);
                }
                break;
            
            //红绿灯及其状态的信息
            case RDB_PKG_ID_TRAFFIC_LIGHT:
                if (sParseMessageEntry) {
                    handleRDBitem(simTime, simFrame, *((RDB_TRAFFIC_LIGHT_t*) dataPtr), 
                        entryHdr->flags& RDB_PKG_FLAG_EXTENDED, rdbMsgNeed);
                }
                break;
            /*      
            case RDB_PKG_ID_SYNC:
                std::cout << "RDB_SYNC_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_DRIVER_PERCEPTION:
                std::cout << "RDB_DRIVER_PERCEPTION_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_TONE_MAPPING:
                std::cout << "RDB_FUNCTION_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_ROAD_QUERY:
                std::cout << "RDB_ROAD_QUERY_t" << std::endl;
                break;
                    
            case RDB_PKG_ID_TRAJECTORY:
                std::cout << "RDB_TRAJECTORY_t" << std::endl;
                break;

            case RDB_PKG_ID_DYN_2_STEER:
                std::cout << "RDB_DYN_2_STEER_t" << std::endl;
                break;

            case RDB_PKG_ID_STEER_2_DYN:
                std::cout << "RDB_STEER_2_DYN_t" << std::endl;
                break;

            case RDB_PKG_ID_PROXY:
                std::cout << "RDB_PROXY_t" << std::endl;
                break;
            */
            default:
                printedMsg = false;
                break;
        }
        dataPtr += entryHdr->elementSize;
    }
    return 2;
}

//处理交通车信息
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
        RDB_OBJECT_STATE_t& item, bool isExtended, RDB_MSG_NEED& rdbMsgNeed)
{
    std::cout << "handleRDBitem: handling object state" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    object = " << item.base.name << ", id = " << item.base.id << std::endl;
    std::cout << "    position = " << item.base.pos.x << " / "
                << item.base.pos.y << " / " << item.base.pos.z << std::endl;
    if (isExtended) {
        std::cout << "    speed = " << item.ext.speed.x << " / " 
                << item.ext.speed.y << " / " << item.ext.speed.z << std::endl;
    }

    if (item.base.id == 1) {
        rdbMsgNeed.egoCar = item;
    } else if (isExtended) {
        rdbMsgNeed.dynamicObjects.emplace_back(item);
    } else {
        rdbMsgNeed.staticObjects.emplace_back(item);
    }
}

//给定实体的详细道路位置信息提取
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
                    RDB_ROAD_POS_t & item, RDB_MSG_NEED& rdbMsgNeed) {
    std::cout << "handleRDBitem: handling road position" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    roadId = " << item.roadId << ", playerId = " << item.playerId << std::endl;
    rdbMsgNeed.roadPos.emplace_back(item);
}

//给定实体的车道信息提取
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
                    RDB_LANE_INFO_t & item, RDB_MSG_NEED& rdbMsgNeed) {
    std::cout << "handleRDBitem: handling lane info" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    laneId = " << item.id << ", playerId = " << item.playerId << std::endl;
    rdbMsgNeed.laneInfo.emplace_back(item);
}

//交通车（通常是主车）的道路标记信息提取
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
                    RDB_ROADMARK_t & item, RDB_MSG_NEED& rdbMsgNeed) {
    std::cout << "handleRDBitem: handling road mark" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    roadMarkId = " << item.id << ", playerId = " << item.playerId << std::endl;
    rdbMsgNeed.roadMark.emplace_back(item);
}

//给定接触点的道路信息提取
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
                    RDB_CONTACT_POINT_t & item, RDB_MSG_NEED& rdbMsgNeed) {
    std::cout << "handleRDBitem: handling contact point" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    contactPointId = " << item.id << ", playerId = " << item.playerId << std::endl;
    rdbMsgNeed.contactPoint.emplace_back(item);
}

//关于交通标志对象的信息提取
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
                    RDB_TRAFFIC_SIGN_t & item, RDB_MSG_NEED& rdbMsgNeed) {
    std::cout << "handleRDBitem: handling traffic sign" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    signalId = " << item.id << ", playerId = " << item.playerId << std::endl;
    rdbMsgNeed.trafficSign.emplace_back(item);
}

//给定交通参与者的道路状态信息提取
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
                    RDB_ROAD_STATE_t & item, RDB_MSG_NEED& rdbMsgNeed) {
    std::cout << "handleRDBitem: handling road state" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    roadlId = " << item.roadId << ", playerId = " << item.playerId << std::endl;
    rdbMsgNeed.roadState.emplace_back(item);
}

//环境信息提取
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
                    RDB_ENVIRONMENT_t & item, RDB_MSG_NEED& rdbMsgNeed) {
    std::cout << "handleRDBitem: handling environment" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    mounth = " << item.month << ", date = " << item.day << std::endl;
    rdbMsgNeed.environment.emplace_back(item);
}

//来自模型或驱动模块的动态输入信息提取
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
                    RDB_DRIVER_CTRL_t & item, RDB_MSG_NEED& rdbMsgNeed) {
    std::cout << "handleRDBitem: handling driver control" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    accelTgt = " << item.accelTgt << ", playerId = " << item.playerId << std::endl;
    rdbMsgNeed.driverCtrl.emplace_back(item);
}
//红绿灯及其状态的信息提取
void handleRDBitem(const double& simTime, const unsigned int& simFrame, 
                    RDB_TRAFFIC_LIGHT_t & item, bool isExtended, RDB_MSG_NEED& rdbMsgNeed) {
    std::cout << "handleRDBitem: handling traffic light" << std::endl;
    std::cout << "    simTime = " << simTime << ", simFrame = " << simFrame << std::endl;
    std::cout << "    isExtended = " << isExtended << ", lightId = " << item.base.id << std::endl;
    rdbMsgNeed.trafficLight.emplace_back(item);
}

//反控主车函数
void sendTrigger(int& sendSocket, const double& simTime, const unsigned int& simFrame)
{
  Framework::RDBHandler myHandler;

  myHandler.initMsg();

  RDB_TRIGGER_t *myTrigger = (RDB_TRIGGER_t*) myHandler.addPackage(simTime, simFrame, RDB_PKG_ID_TRIGGER);

  if (!myTrigger)
    return;

  myTrigger->frameNo = simFrame + 1;
  myTrigger->deltaT  = 0.043;

  int retVal = send( sendSocket, ( const char* ) ( myHandler.getMsg() ), myHandler.getMsgTotalSize(), 0 );

  if ( !retVal )
    fprintf( stderr, "sendTrigger: could not send trigger\n" );

}